#include "Servos.h"
#include <boost/thread/xtime.hpp>
#include <boost/function.hpp>
#include <iomanip>

#define PAN_SERVO_NUM			1
#define INTERVAL_MS		200

Servos::Servos(int ResetGpioNumber,short ComNum, int BaudRate):
SerialWrapper(ComNum, BaudRate),
m_ResetGpioNumber(ResetGpioNumber),
m_shallStop(false),
m_Thread(NULL)
{
	std::stringstream gpioNumberStr;
	gpioNumberStr << m_ResetGpioNumber;

	int ExportFd = open("/sys/class/gpio/export", O_WRONLY);
	write(ExportFd, gpioNumberStr.str().c_str(), gpioNumberStr.str().size());
	close(ExportFd);

	std::stringstream DirectionFileName;
	DirectionFileName << "/sys/class/gpio/gpio" << m_ResetGpioNumber << "/direction";

	int DirectionFd = open(DirectionFileName.str().c_str(), O_WRONLY);
	write(DirectionFd, "out", 3);
	close(DirectionFd);

	char c = '1';
	std::stringstream FileName;
	FileName << "/sys/class/gpio/gpio" << m_ResetGpioNumber << "/value";
	m_ResetGpioFd = open(FileName.str().c_str(),O_WRONLY);
	write(m_ResetGpioFd, &c, 1);
	close(m_ResetGpioFd);

	m_PanServo = new ServoControler(this, PAN_SERVO_NUM);
}

Servos::~Servos()
{
	this->Stop();
	delete m_PanServo;
	ResetActuartors();
}

void Servos::ResetActuartors()
{
	std::stringstream FileName;
	FileName << "/sys/class/gpio/gpio" << m_ResetGpioNumber << "/value";

	char c = '0';

	m_ResetGpioFd = open(FileName.str().c_str(),O_WRONLY);
	write(m_ResetGpioFd, &c, 1);
	close(m_ResetGpioFd);

	boost::this_thread::sleep(boost::posix_time::milliseconds(10));

	c = '1';
	m_ResetGpioFd = open(FileName.str().c_str(),O_WRONLY);
	write(m_ResetGpioFd, &c, 1);
	close(m_ResetGpioFd);
}

void Servos::WriteToBus(char* Message, int MessageSize)
{
	if(false == m_HandleOpened)
		return;

	write(m_Handle, Message, MessageSize);
}

void Servos::Run()
{
	m_shallStop = false;
	boost::function<void (void)>Func = boost::bind(&Servos::IterationsThread, this);
	m_Thread = new boost::thread(Func);
}

void Servos::IterationsThread()
{
	while(false == m_shallStop)
	{
		m_PanServo->Iteration();
		boost::this_thread::sleep(boost::posix_time::milliseconds(INTERVAL_MS));
	}
}

void Servos::Stop()
{
	m_shallStop = true;
	if(NULL != m_Thread)
	{
		m_Thread->join();
		delete m_Thread;
	}
	std::cout << "Servos Stopped!" << std::endl;
}
